#include "bezierPath.h"
#include <math.h>
#include <iostream>
#include <assert.h>
using namespace std;

bezierPath::controllerPoint::controllerPoint(){}

bezierPath::controllerPoint::controllerPoint(Vector3 position, float time)
{
	this->position=position;
	this->time=time;
}

bezierPath::bezierPath():time(this,0), position(this), firstDerivative(this), secondDerivative(this), m_loop(false), n_numberOfLoop(0),
							m_relativeTime(0),m_firstLoop(true)
{
	registerDependent(&position);
	registerDependent(&firstDerivative);
	registerDependent(&secondDerivative);
	currentControllerPoint=-1;
}

void bezierPath::addControllerPoint(Vector3 position1, float time1, Vector3 position2, float time2, 
									Vector3 position3, float time3, Vector3 position4, float time4)
{
	assert(time1<time2);
	assert(time2<time3);
	assert(time3<time4);

	this->vectorControllerPoint.push_back(controllerPoint(position1, time1));
	this->vectorControllerPoint.push_back(controllerPoint(position2, time2));
	this->vectorControllerPoint.push_back(controllerPoint(position3, time3));
	this->vectorControllerPoint.push_back(controllerPoint(position4, time4));
	
	
}

void bezierPath::doUpdate()
{
	float currentTime;
	float oneMinusTime;
	if ((time>= vectorControllerPoint[0].time && time<=vectorControllerPoint[vectorControllerPoint.size()-1].time) ||m_loop)
	{
		if(time>= vectorControllerPoint[0].time && time<=vectorControllerPoint[vectorControllerPoint.size()-1].time)
		{
			if(currentControllerPoint==-1)
			{
				currentControllerPoint=0;
			}
			else if(time>vectorControllerPoint[currentControllerPoint+3].time)
			{
				currentControllerPoint+=4;
			}
			currentTime=(time-vectorControllerPoint[currentControllerPoint].time)/
				(vectorControllerPoint[currentControllerPoint+3].time-vectorControllerPoint[currentControllerPoint].time);
			oneMinusTime=(vectorControllerPoint[currentControllerPoint+3].time-time)/
				(vectorControllerPoint[currentControllerPoint+3].time-vectorControllerPoint[currentControllerPoint].time);

			m_relativeTime=vectorControllerPoint[vectorControllerPoint.size()-1].time;
		}
		else
		{
			if(m_firstLoop)
			{
				n_numberOfLoop++;
				currentControllerPoint=0;
				m_firstLoop=false;
			}
			m_relativeTime=time.getValue()-n_numberOfLoop*(vectorControllerPoint[vectorControllerPoint.size()-1].time-vectorControllerPoint[0].time);
			if(m_relativeTime>=vectorControllerPoint[vectorControllerPoint.size()-1].time)
			{
				n_numberOfLoop++;
				currentControllerPoint=0;
				m_relativeTime=time.getValue()-n_numberOfLoop*(vectorControllerPoint[vectorControllerPoint.size()-1].time-vectorControllerPoint[0].time);
			}				

			
			if(m_relativeTime>vectorControllerPoint[currentControllerPoint+3].time)
			{
				currentControllerPoint+=4;
			}
			currentTime=(m_relativeTime-vectorControllerPoint[currentControllerPoint].time)/
				(vectorControllerPoint[currentControllerPoint+3].time-vectorControllerPoint[currentControllerPoint].time);
			oneMinusTime=(vectorControllerPoint[currentControllerPoint+3].time-m_relativeTime)/
				(vectorControllerPoint[currentControllerPoint+3].time-vectorControllerPoint[currentControllerPoint].time);
		}
	
		Vector3 point1=vectorControllerPoint[currentControllerPoint].position;
		Vector3 point2=vectorControllerPoint[currentControllerPoint+1].position;
		Vector3 point3=vectorControllerPoint[currentControllerPoint+2].position;
		Vector3 point4=vectorControllerPoint[currentControllerPoint+3].position;

		float oneMinusTimePower2=oneMinusTime*oneMinusTime;
		float oneMinusTimePower3=oneMinusTimePower2*oneMinusTime;
		float currentTimePower2=currentTime*currentTime;
		float currentTimePower3=currentTimePower2*currentTime;

		Vector3 position;
		position.x = oneMinusTimePower3*point1.x+3*currentTime*oneMinusTimePower2*point2.x+
			3*currentTimePower2*oneMinusTime*point3.x+currentTimePower3*point4.x;
		
		position.y = oneMinusTimePower3*point1.y+3*currentTime*oneMinusTimePower2*point2.y+
			3*currentTimePower2*oneMinusTime*point3.y+currentTimePower3*point4.y;

		position.z = oneMinusTimePower3*point1.z+3*currentTime*oneMinusTimePower2*point2.z+
			3*currentTimePower2*oneMinusTime*point3.z+currentTimePower3*point4.z;

		//cout<<"position --bezier x="<<position.x<<" y="<<position.y<<" z="<<position.z<<endl;
		Vector3 firstDerivative;
		firstDerivative.x=3*(point2.x-point1.x)*oneMinusTimePower2+6*(point3.x-point2.x)*(currentTime-currentTimePower2)+
			3*(point4.x-point3.x)*currentTimePower2;

		firstDerivative.y=3*(point2.y-point1.y)*oneMinusTimePower2+6*(point3.y-point2.y)*(currentTime-currentTimePower2)+
			3*(point4.y-point3.y)*currentTimePower2;

		firstDerivative.z=3*(point2.z-point1.z)*oneMinusTimePower2+6*(point3.z-point2.z)*(currentTime-currentTimePower2)+
			3*(point4.z-point3.z)*currentTimePower2;

		Vector3 secondDerivative(1,0,0);
		secondDerivative.x=(point2.x-point1.x)*(-6+6*currentTime)+(point3.x-point2.x)*(6-12*currentTime)+(point4.x-point3.x)*6*currentTime;
		secondDerivative.y=(point2.y-point1.y)*(-6+6*currentTime)+(point3.y-point2.y)*(6-12*currentTime)+(point4.y-point3.y)*6*currentTime;
		secondDerivative.z=(point2.z-point1.z)*(-6+6*currentTime)+(point3.z-point2.z)*(6-12*currentTime)+(point4.z-point3.z)*6*currentTime;


		float magnitudeSecondDerivative=tsg::math::length(secondDerivative);	
		if(secondDerivative.y<0)
		{
			secondDerivative.y*=-1;
		}

		if(magnitudeSecondDerivative==0)
		{
			secondDerivative.x=0;
			secondDerivative.y=0;
			secondDerivative.z=-1;
		}
	
		//cout<<"curvature Bezier --> x="<<secondDerivative.x<<" y="<<secondDerivative.y<<" z="<<secondDerivative.z<<endl;

		this->position.setValue(position);
		this->firstDerivative.setValue(firstDerivative);
		this->secondDerivative.setValue(secondDerivative);
	}

}